
#include <string.h>
#include "bsp_pid.h"


/*
    bsp_pid使用方法：
        首先在头文件中选择要使用什么类型的pid
        接着调用pid_init()函数，将pid参数写入其中
        然后调用pid_calculate()函数，输入目标量和当前量
        即可计算出结果

        位置式输出的是直接的结果，与上次的输出结果没关系
        增量式输出的是与上次结果的差值，需要在上次的输出结果上修改
*/

/*
    pid初始化
    参数: pid结构体，三种系数
    返回值： 成功返回0
*/
int pid_init (pid_t *pid, float kp, float ki, float kd)
{
    memset(pid, 0, sizeof(pid_t));

    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;

    return 0;
}

/*
    pid计算公式，增量式和位置式
    参数： pid结构体，目标量，当前量
    返回值： 计算结果
*/
float pid_calculate (pid_t *pid, float SetValue, float ActualValue)
{
    float ret;
    pid->cur_err = SetValue - ActualValue;

    #ifdef USE_PID_INC
        #ifdef INC_EAZY
            /*
            增量公式为 Δu=Kp*{e(n)-e(n-1)}+Ki*e(n)+Kd*{e(n)-2*e(n-1)+e(n-2)}
            整理为 Δu=（Kp+Ki+Kd)*e(n)-(Kp+2Kd)*e(n-1)+Kd*e(n-2)
            将前面的汇集成一个参数，为 Δu=KP*e(n)-KI*e(n-1)+KD*e(n-2)
            */
            ret = pid->Kp*pid->cur_err - pid->Ki*pid->pre_err + pid->Kd*pid->pre2_err;
        #else
            ret = pid->Kp*(pid->cur_err-pid->pre_err) +\
                    pid->Ki*pid->cur_err +\
                    pid->Kd*(pid->cur_err-2*pid->pre_err+pid->pre2_err);
        #endif
        pid->pre2_err = pid->pre_err;
        pid->pre_err = pid->cur_err;
    #endif

    #ifdef USE_PID_POS
        pid->locSum += pid->cur_err;
        ret = pid->Kp*pid->cur_err + pid->Ki*pid->locSum + pid->Kd*(pid->cur_err-pid->pre_err);
        pid->pre_err = pid->cur_err;
    #endif

    return ret;
}
